#pragma once

#include "CameraPoseVisualization.h"
#include "vins_so/estimator/estimator.h"
#include "vins_so/estimator/vins_parameters.h"
#include <eigen3/Eigen/Dense>
#include <fstream>
#include <geometry_msgs/PointStamped.h>
#include <iostream>
#include <nav_msgs/Odometry.h>
#include <nav_msgs/Path.h>
#include <ros/ros.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/Imu.h>
#include <sensor_msgs/PointCloud.h>
#include <sensor_msgs/image_encodings.h>
#include <std_msgs/Float32.h>
#include <std_msgs/Header.h>
#include <tf/transform_broadcaster.h>
#include <visualization_msgs/Marker.h>

extern ros::Publisher pub_odometry;
extern ros::Publisher pub_path, pub_pose;
extern ros::Publisher pub_cloud, pub_map;
extern ros::Publisher pub_key_poses;
extern ros::Publisher pub_ref_pose, pub_cur_pose;
extern ros::Publisher pub_key;
extern nav_msgs::Path path;
extern ros::Publisher pub_pose_graph;

extern int IMAGE_ROW, IMAGE_COL;

void
registerPub( ros::NodeHandle& n );

void
pubLatestOdometry( const Eigen::Vector3d& P,
                   const Eigen::Quaterniond& Q,
                   const Eigen::Vector3d& V,
                   const std_msgs::Header& header );

void
printStatistics( const Estimator& estimator, double t );

void
pubOdometry( const Estimator& estimator,
             const std_msgs::Header& header,
             Eigen::Vector3d loop_correct_t,
             Eigen::Matrix3d loop_correct_r );

void
pubInitialGuess( const Estimator& estimator, const std_msgs::Header& header );

void
pubKeyPoses( const Estimator& estimator,
             const std_msgs::Header& header,
             Eigen::Vector3d loop_correct_t,
             Eigen::Matrix3d loop_correct_r );

void
pubCameraPose( const Estimator& estimator,
               const std_msgs::Header& header,
               Eigen::Vector3d loop_correct_t,
               Eigen::Matrix3d loop_correct_r );

void
pubPointCloud( const Estimator& estimator,
               const std_msgs::Header& header,
               Eigen::Vector3d loop_correct_t,
               Eigen::Matrix3d loop_correct_r );

void
pubPoseGraph( CameraPoseVisualization* posegraph, const std_msgs::Header& header );

void
updateLoopPath( nav_msgs::Path _loop_path );

void
pubTF( const Estimator& estimator,
       const std_msgs::Header& header,
       Eigen::Vector3d loop_correct_t,
       Eigen::Matrix3d loop_correct_r );
